// simplescenegraph.cpp
//

#include <GL/freeglut.h> // include freeglut before stdlib
#include <stdlib.h>
#include <stdio.h>

#include <time.h>

#include "siut\simd\Mat4f.hpp"
#include "siut\simd\Vec4f.hpp"
#include "siut\simd\Vec3f.hpp"

using siut::simd::Mat4f;
using siut::simd::Vec4f;
using siut::simd::Vec3f;

// Spatial contains local and world transforms. if it has a parent, it will recieve the parents transform by multiplying local with parent.
class Spatial
{
public: 
  Spatial();
  virtual ~Spatial();

  virtual void UpdateWorldTransform();
  virtual void Render() = 0;

  void setScale(float s){mLocalScale = s;}
  //void setScale(Vec4f s){mLocalScale = s; mLocalScale.normalize(); }
  void setLocalRotation(Mat4f r){ mLocalRotate = r; }
  void setLocalTranslation(Vec4f t){ mLocalTranslate = t; }

  Mat4f getLocalRotation(){ return mLocalRotate; }
  Vec4f getLocalTranslation(){ return mLocalTranslate; }

  const Spatial* getParent() const { return mParent; }
  void setParent(Spatial* parent) { mParent = parent; }  
protected:
  Spatial* mParent;

  float mLocalScale;
  Mat4f mLocalRotate;
  Vec4f mLocalTranslate;

  float mWorldScale;
  Mat4f mWorldRotate;
  Vec4f mWorldTranslate;
};

Spatial::Spatial()
{
  mParent = 0x0;

  mLocalScale = 1.0f;
  setIdentity(mLocalRotate);
  mLocalTranslate = Vec4f(0.0f);

  mWorldScale = 1.0f;
  setIdentity(mWorldRotate);
  mWorldTranslate = Vec4f(0.0f); 
}
Spatial::~Spatial(){}

void Spatial::UpdateWorldTransform()
{
  if(mParent)
  {
	mWorldScale = mParent->mWorldScale * mLocalScale;
	mWorldRotate = mParent->mWorldRotate * mLocalRotate;
	mWorldTranslate = mParent->mWorldTranslate + 
					  mParent->mWorldScale * mParent->mWorldRotate * mLocalTranslate;
  }
  else
  {
	mWorldScale = mLocalScale;
	mWorldRotate = mLocalRotate;
	mWorldTranslate = mLocalTranslate;
  }
}

class Node : public Spatial
{
public:
  Node(unsigned int numChildren);
  virtual ~Node();

  virtual void UpdateWorldTransform();
  virtual void Render();
  
  void setChild(unsigned int index, Spatial* child);


protected:
  unsigned int mNumChildren;
  Spatial** mChildren;
};

Node::Node(unsigned int numChildren) : Spatial()
{
  mNumChildren = numChildren;

  mChildren = new Spatial*[mNumChildren];

  
  for(unsigned int i=0; i < mNumChildren; i++)
	  mChildren[i] = 0x0;
}
Node::~Node()
{
  for (unsigned int i = 0; i < mNumChildren; i++)
  {
      if (mChildren[i])
      {
          mChildren[i]->setParent(0);
          delete mChildren[i];
      }
  }

  delete[] mChildren;   
}

void Node::UpdateWorldTransform()
{
  Spatial::UpdateWorldTransform();

  for(unsigned int i=0; i < mNumChildren; ++i)
  {
	mChildren[i]->UpdateWorldTransform();
  }
}

void Node::Render()
{
  for(unsigned int i=0; i < mNumChildren; ++i)
  {
	if(mChildren[i])
	  mChildren[i]->Render();
  }
}

void Node::setChild(unsigned int index, Spatial* child)
{
    // must detach this as the parent of the current child at the given index
    if (mChildren[index])
        mChildren[index]->setParent(0x0);
    
    // set this as the parent of the new child at the given index
    if (child)
        child->setParent(this);
    
    // set the new child
    mChildren[index] = child;
}

class Geometry : public Spatial
{
public:
  Geometry();
  ~Geometry();
  virtual void UpdateWorldTransform();
  virtual void Render();
protected:
  Mat4f transform;
};

Geometry::Geometry() : Spatial() { setIdentity(transform); }
Geometry::~Geometry(){}

void Geometry::UpdateWorldTransform()
{
  Spatial::UpdateWorldTransform();
}

void Geometry::Render()
{
  //Mat4f transform = Mat4f( mWorldRotate );
  transform = Mat4f( mWorldRotate );
  // Multiply all indexes with scale except translation part.
  transform.v_[0 ] *= mWorldScale;
  transform.v_[1 ] *= mWorldScale;
  transform.v_[2 ] *= mWorldScale;
  transform.v_[3 ] *= mWorldScale;
    
  transform.v_[4 ] *= mWorldScale;
  transform.v_[5 ] *= mWorldScale;
  transform.v_[6 ] *= mWorldScale;
  transform.v_[7 ] *= mWorldScale;
    
  transform.v_[8 ] *= mWorldScale;
  transform.v_[9 ] *= mWorldScale;
  transform.v_[10] *= mWorldScale;
  transform.v_[11] *= mWorldScale;
  

  //Mat4f scale(0.0f);
  //scale.v_[0] = mWorldScale[0];
  //scale.v_[5] = mWorldScale[1];
  //scale.v_[10] = mWorldScale[2];
  //scale.v_[15] = 1.0;
  //
  //transform *= scale;

  // Set position
  transform.v_[12] = mWorldTranslate[0];
  transform.v_[13] = mWorldTranslate[1];
  transform.v_[14] = mWorldTranslate[2]; 

  
}


class MightyCube : public Geometry
{
public:
  MightyCube() ;
  virtual ~MightyCube();
  virtual void Render();
};

MightyCube::MightyCube() : Geometry()
{
}
MightyCube::~MightyCube(){}

void MightyCube::Render()
{
  Geometry::Render();
  glPushMatrix();
	glMultMatrixf(&transform[0]);
	glutSolidCube(1);
  glPopMatrix();
}

class Camera
{
	public:
		Mat4f matrix_;
	void update()
	{
    // Make rotation with siut math lib:
	  siut::simd::Mat4f xrotation, yrotation;
	  setIdentity(xrotation); 
    setIdentity(yrotation);
	  rotateX(xrotation,vertiAng); 
    rotateY(yrotation,horizAng);
	  siut::simd::Mat4f rotmat = xrotation * yrotation;

	  pos += dir * rotmat; // Move camera pos with direction vector and rotation
	  dir = siut::simd::Vec4f(0.0); // set direction vector to zero.

	  matrix_ = rotmat;
	  translate(matrix_, pos[0],  pos[1],  pos[2]);
	  
    /*
    // or... Make rotation matrix self:
    siut::simd::Vec3f xrot( cos(horizAng), 0, -sin(horizAng) );
    siut::simd::Vec3f zrot( cos(vertiAng) * sin(horizAng), sin(vertiAng), cos(vertiAng)*cos(horizAng) );
    siut::simd::Vec3f yrot = zrot.cross( xrot ); 
	  siut::simd::Mat4f rotmat( xrot[0], yrot[0], zrot[0], 0,
								xrot[1], yrot[1], zrot[1], 0,
								xrot[2], yrot[2], zrot[2], 0,
								0,0,0, 1);

	  pos += dir * rotmat; // Move camera pos with direction vector and rotation
	  dir = siut::simd::Vec4f(0.0); // set direction vector to zero.
		
	  // Make translation matrix
	  siut::simd::Mat4f tx(
		  1, 0, 0, 0,
		  0, 1, 0, 0,
		  0, 0, 1, 0,
		  pos[0],  pos[1],  pos[2], 1
	  );
	  matrix_ = rotmat * tx; // V = OT, that is, View = Orientation * Translation
    */
	}
	
	float horizAng;
	float vertiAng;
	siut::simd::Vec4f dir;
	siut::simd::Vec4f pos;
};




const int KEY_ID_W        = 0;
const int KEY_ID_S        = 1;
const int KEY_ID_A        = 2;
const int KEY_ID_D        = 3;
const int KEY_ID_SPACE    = 4;
const int KEY_ID_C        = 5;

const int KEY_ID_UP       = 6;
const int KEY_ID_DOWN     = 7;
const int KEY_ID_LEFT     = 8;
const int KEY_ID_RIGHT    = 9;

const int MOUSE_LEFT_BUTTON_DOWN = 20;

double lastframetime;
int window;

bool keyPressed[30];
int mouseX, mouseY; 
int oldMouseX, oldMouseY; 


Spatial *body;
Spatial *bodyGeom;
Spatial *armLeft;
Spatial *armRight;
Spatial *legLeft;
Spatial *legRight;
Spatial *head;

float legAnim = 0.0f; // animate legs with cossin

Camera cam;

void sleep(int ms)
{
  Sleep(ms);
}

double currentTime()
{
  double dTime = GetTickCount() / 1000.0;
  return (dTime);
}

float lerp(float x1, float x2, float t){
	return (x2-x1)*t;
}

float rnd()
{
  return float( rand() ) / float(RAND_MAX-1);
}

void init()
{
  glClearColor(0.0, 0.0, 0.0, 0.0);
  glShadeModel(GL_SMOOTH);
  glEnable(GL_DEPTH_TEST);

  lastframetime = currentTime();

  for(int i=0; i<30; i++)
    keyPressed[i]=false;

  body = new Node(6);
  bodyGeom = new MightyCube();
  armLeft = new MightyCube();
  armRight = new MightyCube();
  legLeft = new MightyCube();
  legRight = new MightyCube();
  head = new MightyCube();

  ((Node*)body)->setChild(0, armLeft);
  ((Node*)body)->setChild(1, armRight);
  ((Node*)body)->setChild(2, legLeft);
  ((Node*)body)->setChild(3, legRight);
  ((Node*)body)->setChild(4, head);
  ((Node*)body)->setChild(5, bodyGeom);

  armLeft->setLocalTranslation( Vec4f(1.9f,0.8f, 0.0f, 1.0f) );
  armRight->setLocalTranslation( Vec4f(-1.9f,0.8f, 0.0f, 1.0f) );

  Mat4f rot; 
  setIdentity(rot);
  rotateZ(rot, 35.0f * (M_PI/180.0f) );
  armLeft->setLocalRotation( rot );
  
  setIdentity(rot);
  rotateZ(rot, -35.0f * (M_PI/180.0f) );
  armRight->setLocalRotation( rot );

  legLeft->setLocalTranslation( Vec4f(1.0f,-3.0f,0.0f, 0) );
  legRight->setLocalTranslation( Vec4f(-1.0f,-3.0f,0.0f, 0) );

  head->setLocalTranslation( Vec4f( 0.0f, 2.5f, 1.0f, 0.0f ) );
  //head->setScale( Vec4f(1.5f) );

  //bodyGeom->setScale( Vec4f(2.0f, 3.0f, 2.0f, 1.0f) );

  //armLeft->setScale( Vec4f(0.5f, 3.0f, 0.5f, 1.0f) );
  //armRight->setScale( Vec4f(0.5f, 3.0f, 0.5f, 1.0f) );

  //legLeft->setScale( Vec4f(0.5f, 3.0f, 0.5f, 1.0f) );
  //legRight->setScale( Vec4f(0.5f, 3.0f, 0.5f, 1.0f) );

  
  body->setLocalTranslation( Vec4f( 0.0f, 4.f, 0.0f, 0.0f ) );
  //body->setLocalRotation( rotBodyGeom );
  body->UpdateWorldTransform();
  
  cam.pos = Vec4f(0,-10,-35.0f,0);
  cam.vertiAng = 25 * M_PI / 180.0f;   
}

void display()
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  double now = currentTime();
  float dt = now - lastframetime;
  lastframetime = now;
  
  //glLoadIdentity();
  glPushMatrix();
  //gluLookAt(0,0,-10, 
  //			0,0,0, 0,1,0);
  cam.update();
  glLoadMatrixf( &cam.matrix_[0] );
  
  body->UpdateWorldTransform();
  body->Render();
  
  glBegin(GL_LINES);
    for(int i=-50; i<50; i++)
    {
      glVertex3f(i,0,50);
      glVertex3f(i+1,0,-50);
      
      glVertex3f(50,0,i);
      glVertex3f(-50,0,i+1);
    }
  glEnd();
  
  glPopMatrix();
  
  // Set Othographic projection and draw text
  glMatrixMode(GL_PROJECTION);
	glPushMatrix();
  glLoadIdentity();
	gluOrtho2D(-1.0f, 1.0f, -1.0f, 1.0f);

  glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glLoadIdentity();

	glRasterPos2f(-1.0f, 0.9f);
	//Print text
	static char textString[32];
	sprintf(textString, "use arrow keys to control figure. WASD + drag mouse(1) to move cam");

	for(unsigned int i=0; i<strlen(textString); ++i)
		glutBitmapCharacter(GLUT_BITMAP_HELVETICA_18, textString[i]);
    
	//reset matrices
	glMatrixMode(GL_PROJECTION);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
  
  
  if( keyPressed[KEY_ID_UP] || keyPressed[KEY_ID_DOWN] )
  {
    body->setLocalTranslation( body->getLocalTranslation() + Vec4f( 0.0f, sinf(2*legAnim)*0.25f, 0.0f, 0.0f ) );
  
  //legLeft->getLocalTranslation()
    legLeft->setLocalTranslation(  Vec4f( 1.0f, -3.0f + sinf(legAnim)*1.0f, 0.0f, 0.0f ) );
    legRight->setLocalTranslation( Vec4f( -1.0f, -3.0f + cosf(legAnim)*1.0f, 0.0f, 0.0f ) );
    legAnim += 5 * dt;
  }
  /*
  Mat4f rot;
  setIdentity(rot);
  rotateY(rot, float(now)  ); 
  body->setLocalRotation( rot );
  */

  /*
  setIdentity(rot);
  rotateX(rot, float(now)  ); 
  legLeft->setLocalRotation( rot );

  setIdentity(rot);
  rotateZ(rot, float(now)  ); 
  legRight->setLocalRotation( rot );
  */
  
  float mouseSpeedX = mouseX - oldMouseX;
  float mouseSpeedY = mouseY - oldMouseY;
  oldMouseX = mouseX;
  oldMouseY = mouseY;
  
  float speed = 10.0f;
  if( keyPressed[KEY_ID_W] ) cam.dir[2] += speed*dt;
  if( keyPressed[KEY_ID_S] ) cam.dir[2] -= speed*dt;
  if( keyPressed[KEY_ID_A] ) cam.dir[0] += speed*dt;
  if( keyPressed[KEY_ID_D] ) cam.dir[0] -= speed*dt;  
  
  if( keyPressed[MOUSE_LEFT_BUTTON_DOWN] )
  {
    cam.horizAng += mouseSpeedX * 0.01;
    cam.vertiAng += mouseSpeedY * 0.01;
  }


  Vec4f newDir = Vec4f( 0.0f, 0.0f, speed*dt, 0.0f ) * inverse( body->getLocalRotation() );

  //if( keyPressed[KEY_ID_UP] ) body->setLocalTranslation( body->getLocalTranslation() + Vec4f( 0.0f, 0.0f, speed*dt, 0.0f ) );
  if( keyPressed[KEY_ID_UP] ) body->setLocalTranslation( body->getLocalTranslation() + newDir );
  if( keyPressed[KEY_ID_DOWN] ) body->setLocalTranslation( body->getLocalTranslation() - newDir );


  Mat4f rotLeft;
  setIdentity(rotLeft);
  rotateY(rotLeft, speed/10*dt);

  Mat4f rotRight;
  setIdentity(rotRight);
  rotateY(rotRight, -speed/10*dt);
  
  if( keyPressed[KEY_ID_LEFT] )  body->setLocalRotation( body->getLocalRotation() * rotLeft );
  if( keyPressed[KEY_ID_RIGHT] ) body->setLocalRotation( body->getLocalRotation() * rotRight );

  glutSwapBuffers();
  glutPostRedisplay();
  sleep(16);
}

void keyDown(unsigned char key, int x, int y)
{
  switch (key) 
  {
    case 'q':
    case 27:
      glutDestroyWindow(window);
#ifndef _WIN32
      // Must use this with regular glut, since it never returns control to main().
      exit(0);
#endif
      break;
      
    case 'w':
      keyPressed[KEY_ID_W] = true;
      break;
    case 'a':
      keyPressed[KEY_ID_A] = true;
      break;
    case 's':
      keyPressed[KEY_ID_S] = true;
      break;
    case 'd':
      keyPressed[KEY_ID_D] = true;
      break;
    case ' ':
      keyPressed[KEY_ID_SPACE] = true;
      break;
    case 'c':
      keyPressed[KEY_ID_C] = true;
      break;
      
    default:
      glutPostRedisplay();
  }
}

void keyUp(unsigned char key, int x, int y)
{
  switch (key) 
  {
    case 'w':
      keyPressed[KEY_ID_W] = false;
      break;
    case 'a':
      keyPressed[KEY_ID_A] = false;
      break;
    case 's':
      keyPressed[KEY_ID_S] = false;
      break;
    case 'd':
      keyPressed[KEY_ID_D] = false;
      break;
    case ' ':
      keyPressed[KEY_ID_SPACE] = false;
      break;
    case 'c':
      keyPressed[KEY_ID_C] = false;
      break;
  }
}

void specialKeyUp(int key,int x,int y)
{
  switch(key)
  {
    case  GLUT_KEY_F1                        : break;
    case  GLUT_KEY_F2                        : break;
    case  GLUT_KEY_F3                        : break;
    case  GLUT_KEY_F4                        : break;
    case  GLUT_KEY_F5                        : break;
    case  GLUT_KEY_F6                        : break;
    case  GLUT_KEY_F7                        : break;
    case  GLUT_KEY_F8                        : break;
    case  GLUT_KEY_F9                        : break;
    case  GLUT_KEY_F10                       : break;
    case  GLUT_KEY_F11                       : break;
    case  GLUT_KEY_F12                       : break;
    case  GLUT_KEY_LEFT                      : keyPressed[KEY_ID_LEFT] = false; break;
    case  GLUT_KEY_UP                        : keyPressed[KEY_ID_UP] = false; break;
    case  GLUT_KEY_RIGHT                     : keyPressed[KEY_ID_RIGHT] = false; break;
    case  GLUT_KEY_DOWN                      : keyPressed[KEY_ID_DOWN] = false; break;
    case  GLUT_KEY_PAGE_UP                   : break;
    case  GLUT_KEY_PAGE_DOWN                 : break;
    case  GLUT_KEY_HOME                      : break;
    case  GLUT_KEY_END                       : break;
    case  GLUT_KEY_INSERT                    : break;                
  }
}

void specialKeyDown(int key,int x,int y)
{
  switch(key)
  {
    case  GLUT_KEY_F1                        : break;
    case  GLUT_KEY_F2                        : break;
    case  GLUT_KEY_F3                        : break;
    case  GLUT_KEY_F4                        : break;
    case  GLUT_KEY_F5                        : break;
    case  GLUT_KEY_F6                        : break;
    case  GLUT_KEY_F7                        : break;
    case  GLUT_KEY_F8                        : break;
    case  GLUT_KEY_F9                        : break;
    case  GLUT_KEY_F10                       : break;
    case  GLUT_KEY_F11                       : break;
    case  GLUT_KEY_F12                       : break;
    case  GLUT_KEY_LEFT                      : keyPressed[KEY_ID_LEFT] = true; break;
    case  GLUT_KEY_UP                        : keyPressed[KEY_ID_UP] = true; break;
    case  GLUT_KEY_RIGHT                     : keyPressed[KEY_ID_RIGHT] = true; break;
    case  GLUT_KEY_DOWN                      : keyPressed[KEY_ID_DOWN] = true; break;
    case  GLUT_KEY_PAGE_UP                   : break;
    case  GLUT_KEY_PAGE_DOWN                 : break;
    case  GLUT_KEY_HOME                      : break;
    case  GLUT_KEY_END                       : break;
    case  GLUT_KEY_INSERT                    : break;                
  }
}

void mousePressed(int button, int state, int posX, int posY)
{
	mouseX = posX;
	mouseY = posY;

  if(button==GLUT_LEFT_BUTTON && state==GLUT_DOWN)
  {
    keyPressed[MOUSE_LEFT_BUTTON_DOWN] = true;
  }  
  if(button==GLUT_LEFT_BUTTON && state==GLUT_UP)
    keyPressed[MOUSE_LEFT_BUTTON_DOWN] = false;
}

void mouseMoved(int posX, int posY)
{
	mouseX = posX;
	mouseY = posY;
}

void mousePassive(int posX, int posY)
{
	mouseX = posX;
	mouseY = posY;
}

void reshape(int w, int h)
{
  glViewport(0, 0, (GLsizei) w, (GLsizei) h); 
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(60.0f, float(w)/float(h) ,1.0f, 3000.0f);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}

int main(int argc, char** argv)
//int _tmain(int argc, _TCHAR* argv[])
{
  glutInit(&argc, argv);
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGB);
  glutInitWindowSize(700, 700); 
  glutInitWindowPosition(10, 10);
  window = glutCreateWindow("simple scenegraph");
  init();
  glutKeyboardFunc(keyDown);
  glutKeyboardUpFunc(keyUp);
  
  glutSpecialFunc(specialKeyDown); // special keys are F1-F12, arrowkeys
  glutSpecialUpFunc(specialKeyUp);
  
  glutReshapeFunc(reshape);
  glutDisplayFunc(display);
  glutMouseFunc(mousePressed);
  glutMotionFunc(mouseMoved);
  glutPassiveMotionFunc(mousePassive);

  // Add other callback functions here..

  glutMainLoop();
  return 0;
}

